﻿#include <pcl/ModelCoefficients.h>
#include <pcl/io/openni2_grabber.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/time.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>


#include <iostream>

#include <windows.h>
#include <string.h>

using namespace std;
typedef pcl::PointXYZ PointT;

pcl::PassThrough<PointT> pass;                         //直通滤波对象
pcl::VoxelGrid<PointT> sor;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
pcl::ExtractIndices<PointT> extract;
pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());

	pcl::console::TicToc tt;

char data[8];
short x, z=0.5;
//short *pre_data[4];


class SimpleOpenNIViewer
{
public:
	SimpleOpenNIViewer() : viewer("PCL OpenNI Viewer") {}  // Construct a cloud viewer, with a window name
	float z_min = 0.265;


	// 定义回调函数cloud_cb_,获取到数据时对数据进行处理
	void cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
	{ 
		if (!viewer.wasStopped())
		{			
			tt.tic();

			pass.setInputCloud(cloud);
			pass.setFilterFieldName("y");
			pass.setFilterLimits(-0.20, 0.20);
			pass.filter(*cloud_filtered);
			
			sor.setInputCloud(cloud_filtered);
			sor.setLeafSize(0.015f, 0.015f, 0.015f);
			sor.filter(*cloud_filtered);
			//std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
		
			ne.setSearchMethod(tree);
			ne.setInputCloud(cloud_filtered);
			ne.setKSearch(50);
			ne.compute(*cloud_normals);
			/*tt.tic();

			pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
			ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
			ne.setMaxDepthChangeFactor(0.02f);
			ne.setNormalSmoothingSize(10.0f);
			ne.setInputCloud(cloud);
			ne.compute(*cloud_normals);
			std::cerr << "  2  Done: " << tt.toc() << " ms\n\n";*/

			seg.setOptimizeCoefficients(true);
			seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
			seg.setNormalDistanceWeight(0.1);
			seg.setMethodType(pcl::SAC_RANSAC);
			seg.setMaxIterations(100);
			seg.setDistanceThreshold(0.03);
			seg.setInputCloud(cloud_filtered);
			seg.setInputNormals(cloud_normals);
			// Obtain the plane inliers and coefficients
			seg.segment(*inliers_plane, *coefficients_plane);
			
			float a, b, c, d;
			short angle, distance;
			if (coefficients_plane->values.size() > 0)
			{
				a = coefficients_plane->values[0];
				b = coefficients_plane->values[1];
				c = coefficients_plane->values[2];
				d = coefficients_plane->values[3];
				angle = atan2(c, a) * 180 / 3.14 - 90;
				distance = (abs(d) / sqrt(a*a + b*b + c*c))*100;
				if(c!=0)cout << "角度: " << angle << endl;
				cout << "距离" << distance;
			}
			//Extract the planar inliers from the input cloud
			extract.setInputCloud(cloud_filtered);
			extract.setIndices(inliers_plane);
			extract.setNegative(false);

			// Write the planar inliers to disk
			pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
			extract.filter(*cloud_plane);
			std::cerr << "    Done: " << tt.toc() << " ms\n\n";

			if(cloud_plane->size()>0)viewer.showCloud(cloud_plane);
		
		}
	}

	void run()
	{
		// create a new grabber for OpenNI devices
		pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

		// make callback function from member function
		boost::function<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
			boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);

		// connect callback function for desired signal
		boost::signals2::connection c = interface->registerCallback(f);

		// start receiving point clouds
		interface->start();

		while (!viewer.wasStopped())
		{
			boost::this_thread::sleep(boost::posix_time::seconds(0.0001));
		}

		// Stop the data acquisition
		interface->stop();
	}

	pcl::visualization::CloudViewer viewer;
};


int main()
{
	//Sleep(10000);
	SimpleOpenNIViewer v;

	v.run();

	return 0;
}